#include "Quaternion.h"
#include <math.h>

const Quaternion Quaternion::NO_ROTATION = Quaternion(1,0,0,0);

Quaternion::Quaternion():
_w(0),_x(0),_y(0),_z(0)
{
}

Quaternion::Quaternion(float w, float x, float y, float z):
_w(w),_x(x),_y(y),_z(z)
{
}

Quaternion::~Quaternion(void)
{
}

// get the unit quaternins from a vector and an angle
void Quaternion::fromAxisAngle(const Vector3<float> &vec, float angle)
{
	// get the normal
	Vector3<float> normal = vec.Normal();
	
	// find the sin
	angle *= 0.5f;
	float sine = sin(angle);

	_w = cos(angle);
	_x = (normal.X() * sine);
	_y = (normal.Y() * sine);
	_z = (normal.Z() * sine);
}

void Quaternion::getAxisAngle(Vector3<float>& axis, double& angle)
{
	float scale = sqrt(_x * _x + _y * _y + _z * _z);
	axis.setX(_x / scale);
	axis.setY(_y / scale);
	axis.setZ(_z / scale);
	angle = acos((double)_w) * 2.0;
}

Quaternion Quaternion::OppositeRotation()
{
	Vector3<float> axis(0,0,0);
	double angle = 0;
	this->getAxisAngle(axis, angle);
	
	return Quaternion::AxisAngle(axis, angle);
}

// magintude of a Quaternion
float Quaternion::magnitude()
{
	return sqrt((_w*_w) + (_x*_x) + (_y*_y) + (_z*_z));
}

// normilize the quaternion if it needs to be
void Quaternion::normalise()
{
	float mag = this->magnitude();
	
	_w /= mag;
	_x /= mag;
	_y /= mag;
	_z /= mag;
}

// return the conjugate of the quaternion 
// if the quaternion is a unit quaternion
Quaternion Quaternion::conjugate() const
{
	return Quaternion(_w, -_x, -_y, -_z);
}

// quaternion multiplication in code
Quaternion Quaternion::operator* (const Quaternion &value) const
{
	// the constructor takes its arguments as (x, y, z, w)
	return Quaternion((_w * value._w) - (_x * value._x) - (_y * value._y) - (_z * value._z), // w
					  (_w * value._x) + (_x * value._w) + (_y * value._z) - (_z * value._y), // x
	                  (_w * value._y) + (_y * value._w) + (_z * value._x) - (_x * value._z), // y
	                  (_w * value._z) + (_z * value._w) + (_x * value._y) - (_y * value._x));// z
}

Quaternion Quaternion::operator*= (const Quaternion &value) 
{
	// the constructor takes its arguments as (x, y, z, w)
	*this = *this * value;
	return *this;
}

// Multiplying a quaternion q with a vector v applies the q-rotation to v
Vector3<float> Quaternion::operator* (const Vector3<float> &vec) const
{
	
	Vector3<float> vn(vec);
	if(vn == Vector3<float>::ZERO)
	{
		return Vector3<float>::ZERO;
	}
	vn = vn.Normal();
	// turn the vector pased in to a normal vector
	Quaternion vQuat, result;
	vQuat._x = vn.X();
	vQuat._y = vn.Y();
	vQuat._z = vn.Z();
	vQuat._w = 0.0f;
	// v' = qvq*
	result = vQuat * this->conjugate();
	result = *this * result;
 
	return (Vector3<float>(result._x, result._y, result._z) * vec.Magnitude());
}

MyMatrix Quaternion::asMatrix()
{
	this->normalise();
	 MyMatrix roto(4,4);
	 // row 1
	 roto.at(0,0) = 1 - 2.0f * (_y*_y + _z*_z);
	 roto.at(0,1) = 2.0f * (_x*_y - _z*_w);
	 roto.at(0,2) = 2.0f * (_x*_z + _y*_w);  
	 // row 2
	 roto.at(1,0) = 2.0f * (_x*_y + _z*_w);  
	 roto.at(1,1) = 1 - 2.0f * (_x*_x + _z*_z);
	 roto.at(1,2) = 2.0f * (_y*_z - _x*_w);
	 // row 3
	 roto.at(2,0) = 2.0f * (_x*_z - _y*_w);
	 roto.at(2,1) = 2.0f * (_y*_z + _x*_w); 
	 roto.at(2,2) = 1 - 2.0f * (_x*_x + _y*_y);


	 roto.at(3,3) = 1;

	 return roto;
}

Quaternion Quaternion::AxisAngle(const Vector3<float> &vec, float angle)
{
	Vector3<float> normal = vec.Normal();

	// find the sin
	angle *= 0.5f;
	float sine = sin(angle);

	return Quaternion(cos(angle), (normal.X() * sine), (normal.Y() * sine), (normal.Z() * sine));
}
	



